#pragma once
#include <vector>
#include <opencv2/core/core.hpp>
#include <math.h>
#include "Segment.h";

using namespace std;
using namespace cv;

const double  FIX_ROUND_ITERATIVE = 0.000001;
const double PI  =3.141592653589793238463;
const int SEGMENT_DIVIDER_FOR_DRAW = 100;
void saveMatrixToFile(const Mat &matrix, const char* roadShapeFileName)
{
	try
	{		
		FILE *pFileCsv;
		 	
		pFileCsv = fopen(roadShapeFileName,"w");
		
		for(int l=0;l<matrix.rows;l++)
		{
			for(int m=0;m<matrix.cols;m++){
				if(m<matrix.cols-1)
					fprintf(pFileCsv, "%f,", matrix.at<double>(l,m));
				else
					fprintf(pFileCsv, "%f", matrix.at<double>(l,m));
			}
			fprintf(pFileCsv,"\n");
		}
		fclose(pFileCsv);
	}
	catch(exception e){
		printf(e.what());
	}
}


void printMatrix(const Mat &matrix)
{
	for(int l=0;l<matrix.rows;l++)
	{
		for(int m=0;m<matrix.cols;m++)
			printf("%.3f\t", matrix.at<double>(l,m));
		printf("\n");
	}
}
void printMatrixLen(const Mat &matrix)
{
	for(int l=0;l<matrix.rows;l++)
	{
		for(int m=0;m<matrix.cols;m++){
			printf("%f\t", matrix.at<double>(l,m));
			if (abs(matrix.at<double>(l,m)) > 0.00000001)
			{
				double x = matrix.at<double>(l,m);
			}
		}
		printf("\n");
	}
}
void printSegments(const vector<Segment*> &segments,const string &outputFileName, const char* prefix = "")
{
	try
	{
		FILE *pFile;

		//--------BEGIN Create *.mlt file------------------------------------------------------------------
		string mltFileName = outputFileName.substr(0, outputFileName.find(".")).append(".mlt");
		pFile = fopen(mltFileName.c_str(),"w");
			
		fprintf(pFile,"newmtl Unknown\n");
		fprintf(pFile,"\tKa \t1 \t1 \t1\n");
		fprintf(pFile,"\tKd \t1 \t1 \t1\n\n");

		fprintf(pFile,"newmtl Straight\n");
		fprintf(pFile,"\tKa \t1 \t1 \t1\n");
		fprintf(pFile,"\tKd \t1 \t0 \t0\n\n");

		fprintf(pFile,"newmtl Circle\n");
		fprintf(pFile,"\tKa \t1 \t1 \t1\n");
		fprintf(pFile,"\tKd \t0 \t1 \t0\n\n");

		fprintf(pFile,"newmtl Spiral\n");
		fprintf(pFile,"\tKa \t1 \t1 \t1\n");
		fprintf(pFile,"\tKd \t0 \t0 \t1\n\n");

		fclose(pFile);
		//--------END Create *.mlt file-----------------------------------------------------------------

		pFile = fopen(outputFileName.c_str(),"w");
		fprintf(pFile,"mtllib %s\n\n", mltFileName.substr( mltFileName.find_last_of("\\") + 1 ).c_str());

		for(int i=0, vertexIndex = 0;i< segments.size();i++ )
		{
			int numberOfVertexForThisSegment = 0;
			Point2d startingPoint = segments.at(i)->startingPoint;
			double startingAngle = segments.at(i)->startingAngle;
			if (segments.at(i)->type == SEGTYPE_LINE)
			{
				double length = dynamic_cast<StraightLine *>(segments.at(i))->length;
				
				for(double t = 0;t< length;t+= (length -0.01)/SEGMENT_DIVIDER_FOR_DRAW)
				{
					fprintf(pFile, "v %.3f %.3f 0\n", startingPoint.x + t*cos(startingAngle), startingPoint.y + t*sin(startingAngle));
					vertexIndex++;
					numberOfVertexForThisSegment++;
				}
				fprintf(pFile, "usemtl Straight \n");
			}
			else if(segments.at(i)->type == SEGTYPE_CIRC)
			{
				double endingAngle = dynamic_cast<CircleArc *>(segments.at(i))->endingAngle;
				double radius = dynamic_cast<CircleArc *>(segments.at(i))->radius;
				double sign = 1;
				if(endingAngle < startingAngle) sign = -1;

				double centerX = startingPoint.x - radius*cos(startingAngle - sign*PI/2);
				double centerY = startingPoint.y - radius*sin(startingAngle - sign*PI/2);
				
				if(sign > 0)
					for(double t = startingAngle; t< endingAngle; t+= (endingAngle-startingAngle-0.01)/SEGMENT_DIVIDER_FOR_DRAW)
					{
						fprintf(pFile, "v %.3f %.3f 0 #Angle = %.3f \n", centerX + radius*cos(t- sign*PI/2), centerY + radius*sin(t- sign*PI/2), t);
						vertexIndex++;
						numberOfVertexForThisSegment++;
					}
				else
					for(double t = endingAngle; t< startingAngle; t+= (startingAngle-endingAngle-0.01)/SEGMENT_DIVIDER_FOR_DRAW)
					{
						fprintf(pFile, "v %.3f %.3f 0 #Angle = %.3f \n", centerX + radius*cos(t- sign*PI/2), centerY + radius*sin(t- sign*PI/2), t);
						vertexIndex++;
						numberOfVertexForThisSegment++;
					}
				fprintf(pFile, "usemtl Circle \n");
			}
			fprintf(pFile, "g %s%d\n", prefix, i);
			fprintf(pFile, "l ");
			for(int j=vertexIndex-numberOfVertexForThisSegment+1;j<=vertexIndex;j++)
			{
				fprintf(pFile, "%d ",j);
			}
			fprintf(pFile, "\n");
		}
		fprintf(pFile,"#End of File"); 
		fclose(pFile);
	}
	catch(exception e){
		printf(e.what());
	}
}


double getStartingAngle(Mat fullParams, const vector<Segment*> segments, const int originSegmentIndex, int segmentIndex)
{
	if(segmentIndex == originSegmentIndex)
	{
		return fullParams.at<double>(0,2);
	}
	else
	{
		switch (segments.at(segmentIndex-1)->type)
		{
		case SEGTYPE_LINE:
			return getStartingAngle(fullParams, segments, originSegmentIndex, segmentIndex -1);
			break;
		case SEGTYPE_CIRC:
			return fullParams.at<double>(segmentIndex-1, 3);
			break;
		}
	}
}

Point2d getStartingPoint(Mat fullParams, const vector<Segment*> segments, const int originSegmentIndex, int segmentIndex)
{
	if(segmentIndex == originSegmentIndex)
	{
		return Point2d(fullParams.at<double>(0,0),fullParams.at<double>(0,1));
	}
	else
	{
		if(segments.at(segmentIndex-1)->type == SEGTYPE_LINE){
			Point2d startingPointOfPrevSegment = getStartingPoint(fullParams, segments, originSegmentIndex, segmentIndex -1);
			double angleOfPrevSegment = getStartingAngle(fullParams, segments, originSegmentIndex, segmentIndex -1);
			double len = fullParams.at<double>(segmentIndex-1, 3);
			return Point2d(startingPointOfPrevSegment.x + len*cos(angleOfPrevSegment), startingPointOfPrevSegment.y + len*sin(angleOfPrevSegment));
		}
		else if(segments.at(segmentIndex-1)->type == SEGTYPE_CIRC){ 
			Point2d startingPointOfPrevSegment = getStartingPoint(fullParams, segments, originSegmentIndex, segmentIndex -1);
			double startingAngleOfCircle = getStartingAngle(fullParams, segments, originSegmentIndex, segmentIndex -1);
			
			double endingAngleOfCircle = fullParams.at<double>(segmentIndex-1, 3);
			double radiusOfCircle = fullParams.at<double>(segmentIndex-1, 4);

			double sign = 1;//rotate direction  + non clockwise
			if(endingAngleOfCircle < startingAngleOfCircle) sign = -1; // clockwise

			Point2d	center = Point2d(startingPointOfPrevSegment.x - radiusOfCircle*cos(startingAngleOfCircle - sign*PI/2), startingPointOfPrevSegment.y - radiusOfCircle*sin(startingAngleOfCircle - sign*PI/2));
			return Point2d(center.x + radiusOfCircle*cos(endingAngleOfCircle-sign*PI/2), center.y + radiusOfCircle*sin(endingAngleOfCircle-sign*PI/2));
		}
	}
}

Mat convertToFullParams(const Mat params, vector<Segment*> segments)
{
	Mat fullParams = Mat(segments.size(), 5, CV_64F);
	
	Point2d lastEndingPoint = Point2d(params.at<double>(0,0), params.at<double>(1,0));
	double lastEndingAngle = params.at<double>(2,0);
	int indexOfParams = 2;
	for(int i=0;i< segments.size(); i++ )
	{
		fullParams.at<double>(i, 0) = lastEndingPoint.x;
		fullParams.at<double>(i, 1) = lastEndingPoint.y;
		fullParams.at<double>(i, 2) = lastEndingAngle;

		if (segments.at(i)->type == SEGTYPE_LINE)
		{
			Segment* s = segments.at(i);
			
			indexOfParams++;
			double length = params.at<double>(indexOfParams, 0);
			fullParams.at<double>(i, 3) = length; 

			lastEndingPoint += Point2d(length *cos(lastEndingAngle), length *sin(lastEndingAngle));
		}
		else if(segments.at(i)->type == SEGTYPE_CIRC)
		{
			indexOfParams++;
			double endingAngle = params.at<double>(indexOfParams, 0); 
			fullParams.at<double>(i, 3) = endingAngle;

			double sign = 1;
			if(endingAngle < lastEndingAngle) sign = -1;


			indexOfParams++;
			double radius =  params.at<double>(indexOfParams, 0); 
			fullParams.at<double>(i, 4) = radius; 

			Point2d center = Point2d( lastEndingPoint.x - radius*cos(lastEndingAngle - sign*PI/2), lastEndingPoint.y - radius*sin(lastEndingAngle- sign*PI/2));
			lastEndingPoint = center + Point2d(radius*cos(endingAngle- sign*PI/2),radius*sin(endingAngle- sign*PI/2));
			lastEndingAngle = endingAngle;
		}
	}
	return fullParams;
}

Mat getParamsFromSegments(const vector<Segment*> &segments)
{
	int numberOfNeededParams = 3;
	for(int i = 0;i<segments.size();i++)
	{
		if (segments.at(i)->type == SEGTYPE_LINE) numberOfNeededParams+=1;
		else if (segments.at(i)->type == SEGTYPE_CIRC) numberOfNeededParams+=2;
	}
	
	Mat params = Mat(numberOfNeededParams, 1, CV_64F);

	//initialize param matrix
	int indexOfParams =0;
	params.at<double>(0, 0) = (segments.at(0))->startingPoint.x;
	params.at<double>(1, 0) = (segments.at(0))->startingPoint.y;
	params.at<double>(2, 0) = (segments.at(0))->startingAngle;
	if (segments.at(0)->type == SEGTYPE_LINE)
	{
		params.at<double>(3, 0) = dynamic_cast<StraightLine *>(segments.at(0))->length;
		indexOfParams = 3;
	}
	else if(segments.at(0)->type == SEGTYPE_CIRC)
	{
		CircleArc* arcPtr = dynamic_cast<CircleArc *>(segments.at(0));
		params.at<double>(3, 0) = dynamic_cast<CircleArc *>(segments.at(0))->endingAngle;
		params.at<double>(4, 0) = dynamic_cast<CircleArc *>(segments.at(0))->radius;
		indexOfParams = 4;
	}
	
	for(int i=1;i< segments.size();i++ )
	{
		if (segments.at(i)->type == SEGTYPE_LINE)
		{
			indexOfParams++;
			params.at<double>(indexOfParams, 0) = dynamic_cast<StraightLine *>(segments.at(i))->length;
			
		}
		else if(segments.at(i)->type == SEGTYPE_CIRC)
		{
			indexOfParams++;
			params.at<double>(indexOfParams, 0) = dynamic_cast<CircleArc *>(segments.at(i))->endingAngle;
			double enAngle = dynamic_cast<CircleArc *>(segments.at(i))->endingAngle;
			double radius = dynamic_cast<CircleArc *>(segments.at(i))->radius;

			indexOfParams++;
			params.at<double>(indexOfParams, 0) = dynamic_cast<CircleArc *>(segments.at(i))->radius;
		}
	}
	return params;
}